#pragma config(Hubs,  S1, HTMotor,  HTMotor,  HTMotor,  HTServo)
#pragma config(Sensor, S1,     ,               sensorI2CMuxController)
#pragma config(Motor,  mtr_S1_C1_1,     motorD,        tmotorNormal, openLoop)
#pragma config(Motor,  mtr_S1_C1_2,     motorE,        tmotorNormal, openLoop)
#pragma config(Motor,  mtr_S1_C2_1,     motorF,        tmotorNormal, openLoop)
#pragma config(Motor,  mtr_S1_C2_2,     motorG,        tmotorNormal, openLoop)
#pragma config(Motor,  mtr_S1_C3_1,     motorH,        tmotorNormal, openLoop)
#pragma config(Motor,  mtr_S1_C3_2,     motorI,        tmotorNormal, openLoop)
#pragma config(Servo,  srvo_S1_C4_1,    servo1,               tServoStandard)
#pragma config(Servo,  srvo_S1_C4_2,    servo2,               tServoContinuousRotation)
#pragma config(Servo,  srvo_S1_C4_3,    servo3,               tServoContinuousRotation)
#pragma config(Servo,  srvo_S1_C4_4,    servo4,               tServoStandard)
#pragma config(Servo,  srvo_S1_C4_5,    servo5,               tServoStandard)
#pragma config(Servo,  srvo_S1_C4_6,    servo6,               tServoNone)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

/////////////////////////////////////////////////////////////////////////////////////////////////////
//
//                           Tele-Operation Mode Code Template
//
// This file contains a template for simplified creation of an tele-op program for an FTC
// competition.
//
// You need to customize two functions with code unique to your specific robot.
//
/////////////////////////////////////////////////////////////////////////////////////////////////////

#include "JoystickDriver.c"  //Include file to "handle" the Bluetooth messages.


/////////////////////////////////////////////////////////////////////////////////////////////////////
//
//                                    initializeRobot
//
// Prior to the start of tele-op mode, you may want to perform some initialization on your robot
// and the variables within your program.
//
// In most cases, you may not have to add any code to this function and it will remain "empty".
//
/////////////////////////////////////////////////////////////////////////////////////////////////////
#include "ballhoop definitions.h"


void initializeRobot()
{
  servo[servo4] = BALLHOOP_HOME;// Place code here to sinitialize servos to starting positions.
  servo[servo1] = CLAMP_HOME;
  servo[servo5] = DIVERTER_FRONTCRATE;
  // Sensors are automatically configured and setup by ROBOTC. They may need a brief time to stabilize.

  return;
}


/////////////////////////////////////////////////////////////////////////////////////////////////////
//
//                                         Main Task
//
// The following is the main code for the tele-op robot operation. Customize as appropriate for
// your specific robot.
//
// Game controller / joystick information is sent periodically (about every 50 milliseconds) from
// the FMS (Field Management System) to the robot. Most tele-op programs will follow the following
// logic:
//   1. Loop forever repeating the following actions:
//   2. Get the latest game controller / joystick settings that have been received from the PC.
//   3. Perform appropriate actions based on the joystick + buttons settings. This is usually a
//      simple action:
//      *  Joystick values are usually directly translated into power levels for a motor or
//         position of a servo.
//      *  Buttons are usually used to start/stop a motor or cause a servo to move to a specific
//         position.
//   4. Repeat the loop.
//
// Your program needs to continuously loop because you need to continuously respond to changes in
// the game controller settings.
//
// At the end of the tele-op period, the FMS will autonmatically abort (stop) execution of the program.
//
/////////////////////////////////////////////////////////////////////////////////////////////////////
// I don't know why these are not defined in joystickdriver.c or something.
// The TJoystick structure includes a buttons field that is a bit map of any
// and all buttons that are currently pressed. Button 1 sets bit 1 and so on.
#define button1   0x01
#define button2   0x02
#define button3   0x04
#define button4   0x08
#define button5   0x10
#define button6   0x20
#define button7   0x40
#define button8   0x80
#define button9  0x100
#define button10 0x200
#define button11 0x400
#define button12 0x800

////////////////////////////////////////////////////////////////////////////////
//
//                         scaleJoystick
//
// Function to scale a joystick value to motor power includes dead band at zero
// DO NOT REMOVE IT    see robot power curve (graph) for more info.
// ================
//
///////////////////////////////////////////////////////////////////////////////

int kMaximumPowerLevel = 100;  // Adjust to set max power level to be used.
bool fastMode = true;
// put in all values to speed up operations values represent the presentage of power
static const int nLogScale[32] =
{
 -100, -95, -90, -80, -75, -70, -65, -60, -55, -50, -45, -40, -35, -30, 0, 0,
   0,  35,  40,  45,  50,  55,  60,  65,  70,  75,  80,  85,  90,  95, 100, 100
};
int nScaled;

int scaleJoystick1(int &nJoy1, int nMaxValue = kMaximumPowerLevel)
{
	//
	// This function scales the joystick settings to the appropriate range for
	// controlling a NXT motor.
	//
	// Joystick values range from -128 to +127.
	// Speed/power settings for NXT motors range from -100 to +100
	//
  // offset joystick vaue to get within the range of zero to 255
  //
  nScaled = nJoy1 + 128;
	// nscaled will be between 0 and 31
	nScaled /= 8;
  // :) that is a pointless smiely face
  nScaled = nLogScale[nScaled];
  //only scale if not full power
	if (nMaxValue!=100)
	{
    nScaled *= nMaxValue;
	  nScaled /= 100;
  }
  return nScaled;
}
//
// Tank mode uses both left and right joysticks to drive the robot.
//
void Tank(int y1, int y2)
{
  motor[motorD] = y2;
  motor[motorE] = y1;
  motor[motorF] = y2;
  motor[motorG] = y1;
}


// task that get joystick values
task scanjoystick()
{
  while (true)
 {
  getJoystickSettings(joystick);
  wait10Msec (1);
 }
}

 //task to lift crates
task cratelift()
{
  //servo[servo1] = 127;
  servo[servo2] = 128;
  servo[servo3] = 128;
  short joy1;
  short joy2;
  bool b4press=false;
  bool b4released=true;
  bool clamped = false;
  bool b8press=false;
  bool b8released=true;
  bool backcrate = false;
  while (true)// start botton check loop
  {

	  joy1 = (joystick.joy2_y1);
	  joy2 = (joystick.joy2_y2);

    // Operate Crate Lift
	  if(joy2>50)
			 servo[servo2] = 0; //move up
		else if (joy2<-50)
			 servo[servo2] = 255;//move down
		else
			 servo[servo2] = 128; //turn off

    // Operate Crate Spinner
		if(joy1>50)
			 servo[servo3] = 255;  //spin backwards
	  else if(joy1<-50)
			 servo[servo3] = 0; // spin forwards
		else
			 servo[servo3] = 128; // turn off

	  // Operate Clamp
		// Toggles between Open & Closed position when button4 is depressed
		if(joystick.joy2_Buttons & button4)
		{
		  b4press=true;
		}
		else
		  b4press=false;

		if (b4press==true) // --b4 pressed
	  {
	    if (b4released==true) // b4 just pressed
	    {
	      b4released=false;
	      if (clamped==true) // was clamped, now unclamp
	      {
	        servo[servo1] = CLAMP_OPEN; // unclamp crate
		      clamped = false;
	      }
	      else
	      {
	        servo[servo1] = CLAMP_HOME; // clamp crate
		      clamped = true;
	      }                 // was clamped, now unclamp end
	    }
	    // else
	    // {
	    //    pressed for a while
	    // }
	  }
	  else      // -- b4 not pressed
	  {
	    b4released=true;
	  }
	//}  // end of botton check loop
		//wait10Msec(1);
	//}


	  //Operate ball diverter
	  if (joystick.joy2_Buttons & button8)
	  {
	    b8press = true;
	  }
  	else
	    b8press=false;


	  if (b8press==true) // b8 pressed
	  {
	    if (b8released==true) // b8 just pressed
	    {
	      b8released=false;
	      if (backcrate==true) // was back, now front
	      {
	        servo[servo5] = DIVERTER_FRONTCRATE; // front crate
		      backcrate = false;
	      }
	      else
	      {
	         servo[servo5] = DIVERTER_BACKCRATE; // back crate
		      backcrate = true;

	      }
	    }
	    else
	    {
	        // pressed for a while
	    }
	  }
	  else      // b8 not pressed
	  {
	    b8released=true;
	  }
	}  // end of botton check loop
  wait10Msec(1);
}
//
//
//
task main()
{
  int maxPower = 75;
  initializeRobot();

  wait10Msec(2) ;
  servo[servo1] =43;
  wait10Msec(2);
  waitForStart();   // wait for start of tele-op phase
  servo[servo4] = BALLHOOP_START;
  //TDriveType DriveStyle = kDriveTank;
  StartTask (cratelift);
  StartTask (scanjoystick);
  while (true)
  {
    //getJoystickSettings(joystick);  // The joystick global is defined in joystick.c

    if(joystick.joy2_Buttons & button1)//bowling ball hoop up
      servo[servo4] = BALLHOOP_HOME;
    if(joystick.joy2_Buttons & button2)//bowling bowl hoop parallel to ground
      servo[servo4] = BALLHOOP_HOME+BALLHOOP_2FLAT;
    if(joystick.joy2_Buttons & button3)//bowling ball hoop in dropped position
      servo[servo4] = BALLHOOP_HOME+BALLHOOP_RAMP;
    if(joystick.joy2_Buttons & button5)//starting ball lift
      motor[motorH] = 100;
    if(joystick.joy2_Buttons & button6)//ball lift in reverse
      motor[motorH] = -100;
    if (!(joystick.joy2_Buttons & button6)&&(motor[motorH] == -100))//after releasing button ball lift stops
      motor[motorH] = 0;
    if(joystick.joy2_Buttons & button7)//stopping ball lift
      motor[motorH] = 0;
    if(joystick.joy2_TopHat==0)  // up button0 pressed
      motor[motorI] = 100; //release scissor lift
    if(!(joystick.joy2_TopHat==0)&& (motor[motorI] ==100))
      motor[motorI] = 0;//if button is not held and motor moving forward, stop scissor lift
    if(joystick.joy2_TopHat==4) //down button pressed
      motor[motorI] = -100;// lower scissor lift
    if(!(joystick.joy2_TopHat==4)&&(motor[motorI] == -100))
      motor[motorI] = 0;   //if button not held and motor reversing, stop scissor lift


    if (joystick.joy1_Buttons & button6)  // return to normal speed
    {
      maxPower = 85;
    }
    if (joystick.joy1_Buttons & button8)
    {
        maxPower = 100;
    }
    else
      maxPower = 85;

    if(((joystick.joy1_y1 < -64) && (joystick.joy1_y2 > 64)) || ((joystick.joy1_y1 < 64) && (joystick.joy1_y2 > -64)))// :D
  {
    maxPower = 100;
  }

    Tank(scaleJoystick1(joystick.joy1_y1, -maxPower),
         scaleJoystick1(joystick.joy1_y2, maxPower));



    wait10Msec(1);
  }
}
//     ___
//  __|___|__
//  |(O)(O)|
//  \\____//
//   \____/ yabenimo
//
